module RobiDemoP {
	provides {
		interface StdControl;
	}
	uses {
		interface Leds;

		interface MainMotorControl;

		interface Timer<TMilli> as DemoTimer;
	}
}

implementation {

	uint8_t state = 0;
	
	command error_t StdControl.start() {
		call DemoTimer.startPeriodic(2000);
		return SUCCESS;
	}
	
	command error_t StdControl.stop() {
		call DemoTimer.stop();
		return SUCCESS;
	}

	event void DemoTimer.fired() {
		switch (state) {
			case 0:
				call Leds.led0Off();
				call MainMotorControl.setPWM(6000,6000,600);
				break;
			case 1:
				call MainMotorControl.setPID(6000,6000,900);
				break;
			case 2:
				call MainMotorControl.setPID(9000,9000,900);
				break;
			case 3:
				call MainMotorControl.setPID(12000,12000,900);
				break;
			case 4:
				call MainMotorControl.setPID(15000,15000,900);
				break;
			case 5:
				call MainMotorControl.setPID(18000,18000,900);
				break;
			case 6:
//				call MainMotorControl.setPID(21000,21000,900);
				break;

//			case 1:
//				call Leds.led0On();
//				call MainMotorControl.setPID(9000, 9000, 15000);
		}
		state = (state+1) % 10;
	}
}
